#include "p2d_force_steer.h"

#include "script/scripthelper.h"
#include "util/log.h"		    // log
#include "util/common_macros.h"		// macros
#include "util/mmgr.h"		    // Needs to come in early

#include "math/mat4x4.h"

#include "dd/dd_man.h"

#include <cassert>
using namespace part2d;


P2DForceSteer::P2DForceSteer()
{
    m_MaxSpeed = 1.0f;
	m_SteeringForce = 1.0f;
}

P2DForceSteer::~P2DForceSteer()
{

}
P2DForce*	P2DForceSteer::Clone()
{
	P2DForceSteer*	pForce = new P2DForceSteer();
	pForce->m_SteeringForce	= m_SteeringForce;
	pForce->m_MaxSpeed		= m_MaxSpeed;
	pForce->m_Destination	= m_Destination;
	return pForce;
}

bool    P2DForceSteer::Load(LuaPlus::LuaObject& _LObj)
{
    m_SteeringForce		= script::ScriptHelper::GetTableFloat(_LObj,	"SteeringForce", 1.0f);
	m_MaxSpeed			= script::ScriptHelper::GetTableFloat(_LObj,	"MaxSpeed", 1.0f);
	m_Destination		= script::ScriptHelper::GetTableVec2(_LObj,		"Destination");
    return true;
}

bool	P2DForceSteer::RenderDebug(const math::Vec2& GroupCenter)
{
	return true;
}

bool	P2DForceSteer::MoveParticles(const math::Vec2& GroupCenter, Particle2D* _Particles, unsigned int _usCount, float dt)
{
    for (unsigned int i=0; i<_usCount; i++)
    {
        Particle2D* pPart = &_Particles[i];
        if (pPart->Active)
        {
			math::Vec2	desired_velocity = pPart->Pos - m_Destination;
			desired_velocity.Normalize();
			desired_velocity*=m_MaxSpeed;

			math::Vec2 steering = desired_velocity - (pPart->Dir * pPart->Speed);

			// calculate based on current time
			math::Vec2	Speed = (pPart->Dir * pPart->Speed) - (steering * dt * m_SteeringForce);
			pPart->Dir = Speed;
			pPart->Dir.Normalize();
			pPart->Speed =m_MaxSpeed;

        }
    }

	return true;
}
